{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Script to get the jacobian matrix which under the .ipynb format"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sym\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Direct state vector\n",
    "x, y, z, w, l, h, v, a, yaw, sigma, lf_r, w_r, dt = sym.symbols(\"x y z w l h v a yaw sigma lf_r w_r dt\")\n",
    "next_x, next_y, next_z, next_w, next_h, next_l, next_v, next_a, next_yaw, next_sigma = sym.symbols(\"next_x next_y next_z next_w next_h next_l next_v next_a next_yaw next_sigma\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Important formula\n",
    "\n",
    "- $\\beta$ (the slip angle between the velocity and heading of the object) \n",
    "    - $\\beta(\\tau) = tan^{-1}(\\frac{l_r}{\\gamma l} tan(\\delta(\\tau)))$\n",
    "- $lr$ (the distance between the gravity center and the rear tire of the object)\n",
    "    - $lr = l \\cdot w_r \\cdot (1 - lf_r)$\n",
    "- $\\eta$ (the angle between the velocity of the object and the $\\textit{X-Axis}$ of the coordinate system)\n",
    "    - $\\eta = \\theta + \\beta$\n",
    "\n",
    "Limited by the symbolic calculation ability of `sympy`, we manually simplified the calculation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# tmp state vector\n",
    "beta, ry_rate, lf, lr = sym.symbols(\"beta ry_rate lf lr\")\n",
    "vyawt, ita, t = sym.symbols(\"vyawt ita t\")\n",
    "vyawt = yaw + beta + (v * sym.sin(beta) / lr) * ita"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Analytical solutions for displacements can also be obtained"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "next_x = x + sym.integrate(v * sym.cos(vyawt), (ita, 0, dt))\n",
    "next_y = y + sym.integrate(v * sym.sin(vyawt), (ita, 0, dt))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle x + \\begin{cases} dt v \\cos{\\left(\\beta + yaw \\right)} & \\text{for}\\: \\left(\\beta = 0 \\wedge v = 0\\right) \\vee \\left(\\beta = \\pi \\wedge v = 0\\right) \\vee \\left(\\beta = 0 \\wedge \\beta = \\pi \\wedge v = 0\\right) \\vee v = 0 \\\\dt v \\cos{\\left(yaw \\right)} & \\text{for}\\: \\beta = 0 \\\\- dt v \\cos{\\left(yaw \\right)} & \\text{for}\\: \\beta = \\pi \\\\- \\frac{lr \\sin{\\left(\\beta + yaw \\right)}}{\\sin{\\left(\\beta \\right)}} + \\frac{lr \\sin{\\left(\\beta + \\frac{dt v \\sin{\\left(\\beta \\right)}}{lr} + yaw \\right)}}{\\sin{\\left(\\beta \\right)}} & \\text{otherwise} \\end{cases}$"
      ],
      "text/plain": [
       "x + Piecewise((dt*v*cos(beta + yaw), Eq(v, 0) | (Eq(beta, 0) & Eq(v, 0)) | (Eq(beta, pi) & Eq(v, 0)) | (Eq(beta, 0) & Eq(beta, pi) & Eq(v, 0))), (dt*v*cos(yaw), Eq(beta, 0)), (-dt*v*cos(yaw), Eq(beta, pi)), (-lr*sin(beta + yaw)/sin(beta) + lr*sin(beta + dt*v*sin(beta)/lr + yaw)/sin(beta), True))"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "next_x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle y + \\begin{cases} dt v \\sin{\\left(\\beta + yaw \\right)} & \\text{for}\\: \\left(\\beta = 0 \\wedge v = 0\\right) \\vee \\left(\\beta = \\pi \\wedge v = 0\\right) \\vee \\left(\\beta = 0 \\wedge \\beta = \\pi \\wedge v = 0\\right) \\vee v = 0 \\\\dt v \\sin{\\left(yaw \\right)} & \\text{for}\\: \\beta = 0 \\\\- dt v \\sin{\\left(yaw \\right)} & \\text{for}\\: \\beta = \\pi \\\\\\frac{lr \\cos{\\left(\\beta + yaw \\right)}}{\\sin{\\left(\\beta \\right)}} - \\frac{lr \\cos{\\left(\\beta + \\frac{dt v \\sin{\\left(\\beta \\right)}}{lr} + yaw \\right)}}{\\sin{\\left(\\beta \\right)}} & \\text{otherwise} \\end{cases}$"
      ],
      "text/plain": [
       "y + Piecewise((dt*v*sin(beta + yaw), Eq(v, 0) | (Eq(beta, 0) & Eq(v, 0)) | (Eq(beta, pi) & Eq(v, 0)) | (Eq(beta, 0) & Eq(beta, pi) & Eq(v, 0))), (dt*v*sin(yaw), Eq(beta, 0)), (-dt*v*sin(yaw), Eq(beta, pi)), (lr*cos(beta + yaw)/sin(beta) - lr*cos(beta + dt*v*sin(beta)/lr + yaw)/sin(beta), True))"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "next_y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# state transition for heading yaw\n",
    "next_yaw = yaw + beta + (v * sym.sin(beta) / lr) * dt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\beta + \\frac{dt v \\sin{\\left(\\beta \\right)}}{lr} + yaw$"
      ],
      "text/plain": [
       "beta + dt*v*sin(beta)/lr + yaw"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "next_yaw"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "# state transition for constant state vector\n",
    "next_z, next_w, next_h, next_l, next_a, next_sigma, next_v = z, w, h, l, 0, sigma, v"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Jacobian Matrix for state transition function"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "# large beta\n",
    "funcs = sym.Matrix([next_x, next_y, next_z, next_w, next_l, next_h, next_v, next_a, next_yaw, next_sigma])\n",
    "args = sym.Matrix([x, y, z, w, l, h, v, a, yaw, sigma])\n",
    "res = funcs.jacobian(args)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 & \\begin{cases} dt \\cos{\\left(\\beta + yaw \\right)} & \\text{for}\\: \\left(\\beta = 0 \\wedge v = 0\\right) \\vee \\left(\\beta = \\pi \\wedge v = 0\\right) \\vee \\left(\\beta = 0 \\wedge \\beta = \\pi \\wedge v = 0\\right) \\vee v = 0 \\\\dt \\cos{\\left(yaw \\right)} & \\text{for}\\: \\beta = 0 \\\\- dt \\cos{\\left(yaw \\right)} & \\text{for}\\: \\beta = \\pi \\\\dt \\cos{\\left(\\beta + \\frac{dt v \\sin{\\left(\\beta \\right)}}{lr} + yaw \\right)} & \\text{otherwise} \\end{cases} & 0 & \\begin{cases} - dt v \\sin{\\left(\\beta + yaw \\right)} & \\text{for}\\: \\left(\\beta = 0 \\wedge v = 0\\right) \\vee \\left(\\beta = \\pi \\wedge v = 0\\right) \\vee \\left(\\beta = 0 \\wedge \\beta = \\pi \\wedge v = 0\\right) \\vee v = 0 \\\\- dt v \\sin{\\left(yaw \\right)} & \\text{for}\\: \\beta = 0 \\\\dt v \\sin{\\left(yaw \\right)} & \\text{for}\\: \\beta = \\pi \\\\- \\frac{lr \\cos{\\left(\\beta + yaw \\right)}}{\\sin{\\left(\\beta \\right)}} + \\frac{lr \\cos{\\left(\\beta + \\frac{dt v \\sin{\\left(\\beta \\right)}}{lr} + yaw \\right)}}{\\sin{\\left(\\beta \\right)}} & \\text{otherwise} \\end{cases} & 0\\\\0 & 1 & 0 & 0 & 0 & 0 & \\begin{cases} dt \\sin{\\left(\\beta + yaw \\right)} & \\text{for}\\: \\left(\\beta = 0 \\wedge v = 0\\right) \\vee \\left(\\beta = \\pi \\wedge v = 0\\right) \\vee \\left(\\beta = 0 \\wedge \\beta = \\pi \\wedge v = 0\\right) \\vee v = 0 \\\\dt \\sin{\\left(yaw \\right)} & \\text{for}\\: \\beta = 0 \\\\- dt \\sin{\\left(yaw \\right)} & \\text{for}\\: \\beta = \\pi \\\\dt \\sin{\\left(\\beta + \\frac{dt v \\sin{\\left(\\beta \\right)}}{lr} + yaw \\right)} & \\text{otherwise} \\end{cases} & 0 & \\begin{cases} dt v \\cos{\\left(\\beta + yaw \\right)} & \\text{for}\\: \\left(\\beta = 0 \\wedge v = 0\\right) \\vee \\left(\\beta = \\pi \\wedge v = 0\\right) \\vee \\left(\\beta = 0 \\wedge \\beta = \\pi \\wedge v = 0\\right) \\vee v = 0 \\\\dt v \\cos{\\left(yaw \\right)} & \\text{for}\\: \\beta = 0 \\\\- dt v \\cos{\\left(yaw \\right)} & \\text{for}\\: \\beta = \\pi \\\\- \\frac{lr \\sin{\\left(\\beta + yaw \\right)}}{\\sin{\\left(\\beta \\right)}} + \\frac{lr \\sin{\\left(\\beta + \\frac{dt v \\sin{\\left(\\beta \\right)}}{lr} + yaw \\right)}}{\\sin{\\left(\\beta \\right)}} & \\text{otherwise} \\end{cases} & 0\\\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & \\frac{dt \\sin{\\left(\\beta \\right)}}{lr} & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1, 0, 0, 0, 0, 0, Piecewise((dt*cos(beta + yaw), Eq(v, 0) | (Eq(beta, 0) & Eq(v, 0)) | (Eq(beta, pi) & Eq(v, 0)) | (Eq(beta, 0) & Eq(beta, pi) & Eq(v, 0))), (dt*cos(yaw), Eq(beta, 0)), (-dt*cos(yaw), Eq(beta, pi)), (dt*cos(beta + dt*v*sin(beta)/lr + yaw), True)), 0, Piecewise((-dt*v*sin(beta + yaw), Eq(v, 0) | (Eq(beta, 0) & Eq(v, 0)) | (Eq(beta, pi) & Eq(v, 0)) | (Eq(beta, 0) & Eq(beta, pi) & Eq(v, 0))), (-dt*v*sin(yaw), Eq(beta, 0)), (dt*v*sin(yaw), Eq(beta, pi)), (-lr*cos(beta + yaw)/sin(beta) + lr*cos(beta + dt*v*sin(beta)/lr + yaw)/sin(beta), True)), 0],\n",
       "[0, 1, 0, 0, 0, 0, Piecewise((dt*sin(beta + yaw), Eq(v, 0) | (Eq(beta, 0) & Eq(v, 0)) | (Eq(beta, pi) & Eq(v, 0)) | (Eq(beta, 0) & Eq(beta, pi) & Eq(v, 0))), (dt*sin(yaw), Eq(beta, 0)), (-dt*sin(yaw), Eq(beta, pi)), (dt*sin(beta + dt*v*sin(beta)/lr + yaw), True)), 0,  Piecewise((dt*v*cos(beta + yaw), Eq(v, 0) | (Eq(beta, 0) & Eq(v, 0)) | (Eq(beta, pi) & Eq(v, 0)) | (Eq(beta, 0) & Eq(beta, pi) & Eq(v, 0))), (dt*v*cos(yaw), Eq(beta, 0)), (-dt*v*cos(yaw), Eq(beta, pi)), (-lr*sin(beta + yaw)/sin(beta) + lr*sin(beta + dt*v*sin(beta)/lr + yaw)/sin(beta), True)), 0],\n",
       "[0, 0, 1, 0, 0, 0,                                                                                                                                                                                                                                                    0, 0,                                                                                                                                                                                                                                                                                                     0, 0],\n",
       "[0, 0, 0, 1, 0, 0,                                                                                                                                                                                                                                                    0, 0,                                                                                                                                                                                                                                                                                                     0, 0],\n",
       "[0, 0, 0, 0, 1, 0,                                                                                                                                                                                                                                                    0, 0,                                                                                                                                                                                                                                                                                                     0, 0],\n",
       "[0, 0, 0, 0, 0, 1,                                                                                                                                                                                                                                                    0, 0,                                                                                                                                                                                                                                                                                                     0, 0],\n",
       "[0, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                                    1, 0,                                                                                                                                                                                                                                                                                                     0, 0],\n",
       "[0, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                                    0, 0,                                                                                                                                                                                                                                                                                                     0, 0],\n",
       "[0, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                      dt*sin(beta)/lr, 0,                                                                                                                                                                                                                                                                                                     1, 0],\n",
       "[0, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                                    0, 0,                                                                                                                                                                                                                                                                                                     0, 1]])"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "# tiny beta\n",
    "funcs = sym.Matrix([x + (v * dt + a * dt * dt / 2) * sym.cos(yaw), \n",
    "                    y + (v * dt + a * dt * dt / 2) * sym.sin(yaw), \n",
    "                    next_z, next_w, next_l, next_h, \n",
    "                    v + a * dt, a, yaw, next_sigma])\n",
    "res_zero = funcs.jacobian(args)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 & dt \\cos{\\left(yaw \\right)} & \\frac{dt^{2} \\cos{\\left(yaw \\right)}}{2} & - \\left(\\frac{a dt^{2}}{2} + dt v\\right) \\sin{\\left(yaw \\right)} & 0\\\\0 & 1 & 0 & 0 & 0 & 0 & dt \\sin{\\left(yaw \\right)} & \\frac{dt^{2} \\sin{\\left(yaw \\right)}}{2} & \\left(\\frac{a dt^{2}}{2} + dt v\\right) \\cos{\\left(yaw \\right)} & 0\\\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 1 & dt & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1, 0, 0, 0, 0, 0, dt*cos(yaw), dt**2*cos(yaw)/2, -(a*dt**2/2 + dt*v)*sin(yaw), 0],\n",
       "[0, 1, 0, 0, 0, 0, dt*sin(yaw), dt**2*sin(yaw)/2,  (a*dt**2/2 + dt*v)*cos(yaw), 0],\n",
       "[0, 0, 1, 0, 0, 0,           0,                0,                            0, 0],\n",
       "[0, 0, 0, 1, 0, 0,           0,                0,                            0, 0],\n",
       "[0, 0, 0, 0, 1, 0,           0,                0,                            0, 0],\n",
       "[0, 0, 0, 0, 0, 1,           0,                0,                            0, 0],\n",
       "[0, 0, 0, 0, 0, 0,           1,               dt,                            0, 0],\n",
       "[0, 0, 0, 0, 0, 0,           0,                1,                            0, 0],\n",
       "[0, 0, 0, 0, 0, 0,           0,                0,                            1, 0],\n",
       "[0, 0, 0, 0, 0, 0,           0,                0,                            0, 1]])"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "res_zero"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Jacobian Matrix for state transition function"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "mea_funcs = sym.Matrix([x - l * w_r * (0.5 - lf_r) * sym.cos(yaw), \n",
    "                        y - l * w_r * (0.5 - lf_r) * sym.sin(yaw), \n",
    "                        z, w, l, h, \n",
    "                        v * sym.cos(yaw + beta), v * sym.sin(yaw + beta), \n",
    "                        yaw])\n",
    "mea_res = mea_funcs.jacobian(args)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(Matrix([\n",
       " [1, 0, 0, 0, -w_r*(0.5 - lf_r)*cos(yaw), 0,               0, 0,  l*w_r*(0.5 - lf_r)*sin(yaw), 0],\n",
       " [0, 1, 0, 0, -w_r*(0.5 - lf_r)*sin(yaw), 0,               0, 0, -l*w_r*(0.5 - lf_r)*cos(yaw), 0],\n",
       " [0, 0, 1, 0,                          0, 0,               0, 0,                            0, 0],\n",
       " [0, 0, 0, 1,                          0, 0,               0, 0,                            0, 0],\n",
       " [0, 0, 0, 0,                          1, 0,               0, 0,                            0, 0],\n",
       " [0, 0, 0, 0,                          0, 1,               0, 0,                            0, 0],\n",
       " [0, 0, 0, 0,                          0, 0, cos(beta + yaw), 0,           -v*sin(beta + yaw), 0],\n",
       " [0, 0, 0, 0,                          0, 0, sin(beta + yaw), 0,            v*cos(beta + yaw), 0],\n",
       " [0, 0, 0, 0,                          0, 0,               0, 0,                            1, 0]]),\n",
       " Matrix([\n",
       " [1, 0, 0, 0, -w_r*(0.5 - lf_r)*cos(yaw), 0,               0, 0,  l*w_r*(0.5 - lf_r)*sin(yaw), 0],\n",
       " [0, 1, 0, 0, -w_r*(0.5 - lf_r)*sin(yaw), 0,               0, 0, -l*w_r*(0.5 - lf_r)*cos(yaw), 0],\n",
       " [0, 0, 1, 0,                          0, 0,               0, 0,                            0, 0],\n",
       " [0, 0, 0, 1,                          0, 0,               0, 0,                            0, 0],\n",
       " [0, 0, 0, 0,                          1, 0,               0, 0,                            0, 0],\n",
       " [0, 0, 0, 0,                          0, 1,               0, 0,                            0, 0],\n",
       " [0, 0, 0, 0,                          0, 0, cos(beta + yaw), 0,           -v*sin(beta + yaw), 0],\n",
       " [0, 0, 0, 0,                          0, 0, sin(beta + yaw), 0,            v*cos(beta + yaw), 0],\n",
       " [0, 0, 0, 0,                          0, 0,               0, 0,                            1, 0]]))"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "mea_res, mea_res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.16"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
